from gpiozero import DistanceSensor
from gpiozero.pins.pigpio import PiGPIOFactory
from time import sleep
from .motors_gpiozero import *

class Ultrasonic:
    AVOIDANCE_ON = 1
    AVOIDANCE_OFF = 0

    def __init__(self, TriggerPin, EchoPin):
        self.avoid_distance = 30 
        self.sensor = DistanceSensor(echo=EchoPin,trigger=TriggerPin,pin_factory= PiGPIOFactory())
        self.avoidance = False
        self.distance=None
    
    def action(self, cmd_to_ul):
        if (cmd_to_ul == Ultrasonic.AVOIDANCE_ON):
            self.avoidance = True
        else:
            if (cmd_to_ul == Ultrasonic.AVOIDANCE_OFF):
                self.avoidance = False

    def worker(self, robotcar):
    # process: def worker(self, motor,distance):
        """ul worker thread"""
        while True:
            sleep(0.05)
            self.distance= self.sensor.distance*100 # m -> cm
            if (self.avoidance):
                if (robotcar.MOTOR_STATE == RobotCar.FORWARD or robotcar.MOTOR_STATE == RobotCar.LEFT or robotcar.MOTOR_STATE == RobotCar.RIGHT):
                    if  (self.distance<self.avoid_distance):
                        robotcar.action(RobotCar.STOP)
